#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <math.h>
#include "physics.h"


/**
 * Klasa symulatora fizyki.
 * Symulator przeprowadza testy kolizji sfery jednostkowej (tj o promieniu 1)
 * ze scena i oblicza odpowiedz. 
 *
 * @author Krzysztof Klein
 * @version 0.1
 */


/**
 * Zwraca aktualny czas systemowy (liczbe milisekund od startu systemu).
 *
 * @return Aktualny czas systemowy
 */
int Physics::Stats::Time::getTimeValue()
{
	return GetTickCount();
}


/**
 * Resetuje statystyki symulatora
 */
void Physics::resetStats()
{
	stats.simulationCount++;
	stats.lastTime = 0;
	stats.checkTime = 0;
	stats.responseTime = 0;
	stats.polygonsChecked = 0;
	stats.polysRejectedInBoundingSphereCheck = 0;
	stats.polysRejectedInBoundingBoxCheck = 0;
	stats.polysRejectedInFrontFaceTest = 0;
}

/**
 * Oblicza najmniejszy dodatni pierwiastek rownania kwadratowego
 * mniejszy od zadanej wartosci.
 *
 * @param a Wspolczynnik a rownania kwadratowego
 * @param b Wspolczynnik b rownania kwadratowego
 * @param c Wspolczynnik c rownania kwadratowego
 * @param maxValue Maksymalna szukana wartosc pierwiastka
 * @param outValue Zmienna do ktorej zostanie wpisana wartosc pierwiastka
 * @return True jesli znaleziono. flase jesli nie.
 */
bool Physics::getLowestPositiveRoot(float a, float b, float c, float maxValue,
	float &outValue)
{
	// sprawdz czy istnieja rozwiazania rownania kwadratowego
	const float det = b*b - 4.0f*a*c;
	if(det < 0.0f)
		return false;

	// oblicz pierwiastki
	const float sqrtDet = sqrt(det);
	float x1 = (-b - sqrtDet) / (2.0f * a);
	float x2 = (-b + sqrtDet) / (2.0f * a);

	// posortuj pierwiastki zeby x1 <= x2
	if(x1 > x2)
	{
		const float temp = x1;
		x1 = x2;
		x2 = temp;
	}

	// wynik = x1?
	if((x1 > 0.0f) && (x1 < maxValue))
	{
		outValue = x1;
		return true;
	}

	// wynik = x2?
	if((x2 > 0.0f) && (x2 < maxValue))
	{
		outValue = x2;
		return true;
	}

	// brak rozwiazan
	return false;
}

/**
 * Sprawdza czy punkt lezy w wielokacie.
 * Punkt musi lezec na plaszczyznie trojkata.
 *
 * @param poly Wielokat
 * @param point Sprawdzany punkt
 * @return True jesli punkt lezy w wielokacie. False jesli nie.
 */
bool Physics::checkPointInPolygon(const class Polygon &poly, const Vector &point)
{
	// algorytm z http://www.devmaster.net/forums/showthread.php?t=6483
	const std::list<Vector> &vertices = poly.getVertices();
	std::list<Vector>::const_iterator it = vertices.begin();
	Vector p = ((*vertices.rbegin()) - point).crossProduct((*vertices.begin()) - point);
	for(unsigned i = 0; i < vertices.size() - 1; i++)
    {
		Vector a, b;
		a = (*it) - point;
		it++;
		b = (*it) - point;
		Vector q = a.crossProduct(b);
		if(p.dotProduct(q) < 0)
            return false;
    }
    return true;
}


/**
 * Sprawdza kolizje z wielokatem.
 *
 * @param poly Wielokat
 * @param col Pakiet kolizji
 */
void Physics::checkPolygonCollision(const class Polygon &poly, Physics::CollisionPacket &col) const
{
	auto Stats::Time statsTotalTime(&stats.totalTime);
	auto Stats::Time statsLastTime(&stats.lastTime);
	auto Stats::Time statsCheckTime(&stats.checkTime);

	// sprawdz sfery otaczajace?
	if(settings.boundingSphereCheck)
	{
		// sprawdz
		if(!poly.getBoundingSphere().isCollidingWithSphere(col.basePosition,
			1.0f + col.velocity.getLength()))
		{
			stats.polysRejectedInBoundingSphereCheck++;
			return;
		}
	}

	// sprawdz prost. otaczajace?
	if(settings.boundingBoxCheck)
	{
		// sprawdz
		if(!poly.getBoundingBox().isCollidingWithSphere(col.basePosition,
			1.0f + col.velocity.getLength()))
		{
			stats.polysRejectedInBoundingBoxCheck++;
			return;
		}
	}

	const Plane &polygonPlane = poly.getPlane();
	bool foundCollision = false;
	bool embeddedInPlane = false;

	// tylko widoczne sciany?
	if(settings.frontFacesOnly)
	{
		// sciana niewidoczna?
		if(!polygonPlane.isFrontFacing(col.normalizedVelocity))
		{
			// tak - koniec
			stats.polysRejectedInFrontFaceTest++;
			return;
		}
	}

	// sprawdzamy wielokat
	stats.polygonsChecked++;

	// oblicz odleglosc srodka sfery od plaszczyzny wielokata
	const float baseDistance = polygonPlane.dotProduct(col.basePosition);

	// oblicz iloczyn skalarny predkosci i normalnej wielokata (przyda sie wiele razy)
	const float normalDotVelocity = polygonPlane.getNormal().dotProduct(col.velocity);

	// czasy kolizji
	float t, t0, t1;

	// sfera porusza sie rownolegle do wielokata?
	if(fabs(normalDotVelocity) <= settings.precision)
	{
		// tak - sprawdz odleglosc sfery od plaszczyzny
		if(fabs(baseDistance) >= 1.0f)
		{
			// sfera lezy daleko od wielokata - brak kolizji
			return;
		}
		
		// sfera lezy w plaszczyznie wielokata
		embeddedInPlane = true;
		t0 = 0.0f;
		t1 = 1.0f;
	}
	else
	{
		embeddedInPlane = false;

		// oblicz czasy kolizji
		t0 = (-1.0f - baseDistance) / normalDotVelocity;
		t1 = ( 1.0f - baseDistance) / normalDotVelocity;

		// posortuj zeby t0 <= t1
		if(t0 > t1)
		{
			const float temp = t0;
			t0 = t1;
			t1 = temp;
		}

		// sprawdz czy chociaz jeden z czasow jest w przedziale <0, 1>
		if((t0 > 1.0f) || (t1 < 0.0f))
		{
			// nie - brak kolizji
			return;
		}

		// zacisnij czasy do przedzialu <0, 1>
		if(t0 < 0.0f)
			t0 = 0.0f;
		if(t1 > 1.0f)
			t1 = 1.0f;
	}


	// sprawdz kolizje z wnetrzem wielokata
	Vector collisionPoint;
	t = 1.0;
	if(!embeddedInPlane)
	{
		const Vector planeIntersectionPoint =
			(col.basePosition - polygonPlane.getNormal()) + col.velocity*t0;
		if(checkPointInPolygon(poly, planeIntersectionPoint))
		{
			foundCollision = true;
			t = t0;
			collisionPoint = planeIntersectionPoint;
		}
	}

	// Sprawdz kolizje z wierzcholkami i krawedziami
	if(!foundCollision)
	{
		const std::list<Vector> &vertices = poly.getVertices();
		std::list<Vector>::const_iterator it, it_next;	
		float velocityLengthSquared = col.velocity.getLengthSquared();
		float a, b, c;
		float new_t;

		// Sprawdz kolizje z wierzcholkami
		a = velocityLengthSquared;
		for(it = vertices.begin(); it != vertices.end(); it++)
		{
			b = 2.0f * col.velocity.dotProduct(col.basePosition - (*it));
			c = ((*it) - col.basePosition).getLengthSquared() - 1.0f;
			if(getLowestPositiveRoot(a, b, c, t, new_t))
			{
				t = new_t;
				foundCollision = true;
				collisionPoint = (*it);
			}
		}

		// Sprawdz kolizje z krawedziami
		it_next = vertices.begin();
		it_next++;
		for(it = vertices.begin(); it != vertices.end(); it++, it_next++)
		{
			if(it_next == vertices.end())
				it_next = vertices.begin();

			Vector edge = (*it_next) - (*it);
			Vector baseToVertex = (*it) - col.basePosition;
			float edgeLengthSquared = edge.getLengthSquared();
			float edgeDotVelocity =	edge.dotProduct(col.velocity);
			float edgeDotBasetoVertex = edge.dotProduct(baseToVertex);
			a = -edgeLengthSquared*velocityLengthSquared +
				edgeDotVelocity*edgeDotVelocity;
			b = edgeLengthSquared*2.0f*col.velocity.dotProduct(baseToVertex)-
				2.0f*edgeDotVelocity*edgeDotBasetoVertex;
			c = edgeLengthSquared*(1.0f-baseToVertex.getLengthSquared())+
				edgeDotBasetoVertex*edgeDotBasetoVertex;
			if(getLowestPositiveRoot(a, b, c, t, new_t))
			{
				float f = (edgeDotVelocity*new_t-edgeDotBasetoVertex)/edgeLengthSquared;
				if((f >= 0.0f) && (f <= 1.0f))
				{
					t = new_t;
					foundCollision = true;
					collisionPoint = (*it) + edge*f;
				}
			}
		}
	}
	
	// Oblicz dane dla odpowiedzi
	if(foundCollision)
	{
		float distToCollision = t*col.velocity.getLength();
		if((!col.foundCollision) || (distToCollision < col.nearestDistance))
		{
			col.nearestDistance = distToCollision;
			col.intersectionPoint = collisionPoint;
			col.foundCollision = true;
		}
	}
}


/**
 * Oblicza odpowiedz symulatora na podstawie pakietu kolizji
 *
 * @param col Pakiet kolizji
 * @return True jesli koniec symulacji. False jesli nie.
 */
bool Physics::doResponse(Physics::CollisionPacket &col)
{
	auto Stats::Time statsTotalTime(&stats.totalTime);
	auto Stats::Time statsLastTime(&stats.lastTime);
	auto Stats::Time statsResponseTime(&stats.responseTime);

	// sprawdz czy w cokolwiek trafilismy i zapamietaj to
	if(col.foundCollision && !col.hitAnything)
		col.hitAnything = true;

	// przekroczono max iteracji?
	if(col.responseIteration > settings.responseIterations)
	{
		col.finalPosition = col.basePosition;
		return true;
	}

	// punkt docelowy pozycji
	const Vector destinationPoint = col.basePosition + col.velocity;

	// brak kolizji?
	if(!col.foundCollision)
	{
		// standardowe przesuniecie
		col.finalPosition = destinationPoint;
		return true;
	}

	// przysun sfere do odleglosci bliskiej kolizji
	if(col.nearestDistance >= settings.veryShortLength)
	{
		Vector v = col.velocity;
		v.normalize();
		col.basePosition = col.basePosition +
			v * (col.nearestDistance - settings.veryShortLength);
		col.intersectionPoint -= v * settings.veryShortLength;
	}

	// wyznacz plaszczyzne slizgu
	Plane slidingPlane = Plane(col.basePosition - col.intersectionPoint,
		col.intersectionPoint);

	// nowy punkt docelowy pozycji (z uwzglednieniem plaszczyzny slizgu)
	Vector newDestinationPoint = destinationPoint -
		slidingPlane.getNormal() * slidingPlane.dotProduct(destinationPoint);

	// nowy wektor predkosci dla nastepnej iteracji
	col.velocity = newDestinationPoint - col.intersectionPoint;

	if(col.velocity.getLengthSquared() <
		(settings.veryShortLength * settings.veryShortLength))
	{
		// nowy wektor predkosci jest bardzo maly - koniec
		col.finalPosition = col.basePosition;
		return true;
	}

	// kolejna iteracja
	col.responseIteration++;
	col.normalizedVelocity = col.velocity;
	col.normalizedVelocity.normalize();
	col.foundCollision = false;
	return false;
}
